#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import rospy #导入rospy库
import actionlib #导入actionlib 库
import os,inspect #导入os库
from actionlib_msgs.msg import * #导入actionlib的所有模块
from geometry_msgs.msg import Pose, Point, Quaternion, Twist #导入四个消息数据类型，姿态，目标点，四元数，运动消息Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal #导入movebase的两个消息数据类型
from tf.transformations import quaternion_from_euler #导入tf变换库的欧拉角转四元数库
from aikit_arm.srv import aikit_claw_srv
from math import pi #导入圆周率pi
from std_msgs.msg import String #导入标准消息的字符串消息数据格式
import yaml
nav_goals=[]
goal_dict=dict()

result=""
place_name=""
qrcode_data=""
object_data=""
color_data=""

qrcode_data_list=["人工","智能","机器人"]
color_data_list=["red","green","blue"]
object_data_list=["bottle","pottedplant"]

#初始化节点
rospy.init_node('nav_goal_place',anonymous=False)
square_size = 1.0
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) #实例化一个消息发布函数
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) #action服务器连接

#设置参数 
rospy.loginfo('等待move_base action服务器连接...')
move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo('已连接导航服务')

rospy.wait_for_service("aikit_claw")
rospy.loginfo('已连接机械臂控制服务')
claw = rospy.ServiceProxy("aikit_claw", aikit_claw_srv)

yaml_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + "/goal1.yaml"
#添加导航坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
def nav_to(x,y,th):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id='map'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = pose_e(x,y,th)
    move(goal)

#写一个函数 用于任务完成提示。
def move(goal):
    move_base.send_goal(goal)
    finished_within_time = move_base.wait_for_result(rospy.Duration(40))
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo('时间超时，导航任务取消。')
    state = move_base.get_state()
    if state == GoalStatus.SUCCEEDED:
        play("arrive"+place_name)
        rospy.loginfo(place_name+'导航成功！')
    
def shutdown():
    rospy.loginfo('机器人任务停止')
    move_base.cancel_goal()
    rospy.sleep(2)
    cmd_vel_pub.publish(Twist)
    rospy.sleep(1)
    
def pose_e(x,y,th):#输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
    new_pose=Pose()
    new_pose.position.x=float(x)
    new_pose.position.y=float(y)
    #机器朝向，平面朝向弧度转化成四元数空间位姿
    q=quaternion_from_euler(0.0,0.0,float(th)/180.0*pi)
    new_pose.orientation.x=q[0]
    new_pose.orientation.y=q[1]
    new_pose.orientation.z=q[2]
    new_pose.orientation.w=q[3]
    return  new_pose

def yaml_read():
    global goal_dict
    global nav_goals
    f = open(yaml_path) 
    cfg =  f.read()
    goal_dict = yaml.load(cfg)
    nav_goals=goal_dict.keys()

def nav_callback(data):
    g=data.data
    # print(type(g))
    goal=g.decode("utf-8")
    # print(type(goal))
    if goal in nav_goals:
        goal_data=goal_dict[goal]
        nav_to(goal_data[0],goal_data[1],goal_data[2])

def nav_place(place):
    global place_name
    place_name=place
    print("尝试导航去:"+place_name)
    goal=place.decode("utf-8")
    if goal in nav_goals:
        goal_data=goal_dict[goal]
        nav_to(goal_data[0],goal_data[1],goal_data[2])

def play(filepath):
    cmd="play ~/Desktop/wrc_demo/mp3/"+filepath+".mp3"
    os.system(cmd)
   
# def nav_input():
#     rospy.Subscriber("nav_topic",String,nav_callback,queue_size=1)
#     rospy.spin()
def qrcode_callback(data):
    global qrcode_data
    qrcode_data=data.data
    print(qrcode_data)

def object_callback(data):
    global object_data
    object_data=data.data
    print(object_data)

def color_callback(data):
    global color_data
    color_data=data.data
    print(color_data)

def msg_sub():
    rospy.Subscriber("/object_data",String,object_callback,queue_size=1)
    rospy.Subscriber("/qrcode_data",String,qrcode_callback,queue_size=1)
    # rospy.Subscriber("/color_data",String,color_callback,queue_size=1)
def wait_for_cube():
    global result
    global qrcode_data
    global object_data
    global color_data
    result=""
    while True:
        if qrcode_data in qrcode_data_list:
            result=qrcode_data
            qrcode_data=""
            break
        elif object_data in object_data_list:
            result=object_data
            object_data=""
            break
        elif color_data in color_data_list:
            result=color_data
            color_data=""
            break
        else:
            result==""
            color_data=""
            object_data=""
            qrcode_data=""

    play("resultis")
    play(result)
    return result
def take_cube():
    claw(10)
    play("putcube")
    rospy.sleep(3)
    claw(80)
    play("takecube")

def put_cube():
    claw(10)
    play("finishcube")
def clear_data():
    global color_data
    global object_data
    global qrcode_data
    global result
    color_data=""
    object_data=""
    qrcode_data=""
    result=""


if __name__ == "__main__":
    yaml_read()
    msg_sub()
    claw(10)
    play("start")
    for i in range(8):
        nav_place("识别区")
        clear_data()
        name=wait_for_cube()
        take_cube()
        nav_place(name)
        put_cube()
    rospy.spin()
    nav_place("启动区")



        

    # nav_place("智能")
    # nav_place("识别区")
    # nav_place("机器人")
    # nav_place("识别区")
    # nav_place("cat")
    # nav_place("识别区")
    # nav_place("pottedplant")
    # nav_place("启动区")
    



       

        







